// File:          my_controller.cpp
// Date:
// Description:
// Author:  baolan
// Modifications:

// You may need to add webots include files such as
// <webots/DistanceSensor.hpp>, <webots/Motor.hpp>, etc.
// and/or to add some other includes
#include <motionControl.hpp>
#include <robot_locomotion.h>
#include <ctime>
#include <fstream>
#define PI 3.1415926

// All the webots classes are defined in the "webots" namespace
using namespace webots;
MotionControl mc;
rl rl1;

int main(int argc, char **argv) {
    Robot *robot = new Robot();
    Keyboard kb;
    kb.enable(1000);
    float timePeriod = 0.01;
    mc.MotionContr(timePeriod, robot);

    Camera *camera;
    camera=robot->getCamera("camera");
    camera->enable(1000);
    int timeStep = (int)robot->getBasicTimeStep();

    Matrix<float, 4, 3> initPos;
    initPos<<  0.0, 0.0, -0.24, 0.0, 0.0, -0.24, 0.0, 0.0, -0.24, 0.0, 0.0, -0.24;
    mc.setInitPos(initPos);
    mc.tCV<< 0.0, 0.0, 0.0;
    //w:87,s:83,a:65,d:68，q:81,e:69
    mc.setCoMVel(mc.tCV);
    char StateVal;
    StateVal = mc.stateval[0];

    ofstream oFile;
    oFile.open("distance.csv",ios::out | ios::trunc);
  while (robot->step(timeStep) != -1)
  {
    mc.key = kb.getKey();
    mc.Sensor_update();
    mc.forwardKinematics();
    mc.jacobians();
    mc.state_judgement();

    rl1.dsensor[0] =  mc.distance[0];
    rl1.dsensor[1] =  mc.distance[1];
    rl1.dsensor[2] =  mc.distance[2];
    rl1.dsensor[3] =  mc.distance[3];
    rl1.dsensor[4] =  mc.distance[4];
    rl1.dsensor[5] =  mc.distance[5];

    rl1.run();
    rl1.update();
    
    // mc.tCV[0] = rl1.vx;
    // mc.tCV[1] = rl1.vy;
    // mc.tCV[2] = rl1.w;

    switch(StateVal)
    {
      case 'a'://
         mc.State_start();
         if (mc.Start_time >= 0.80)
         {
           StateVal = mc.stateval[1];
           mc.fly_time = 0.0;
          //  mc.imu_start << 0.0, 0.0, 0.0;
           mc.imu_start << 0.0, 0.0, 0.5 * PI;
          //  mc.imu_start = mc.imu_num;
         }
         cout << "a"<<endl;
         break;
      case 'b':// 2 leg stance
        mc.stance_VMC();
        mc.swing_VMC();
        mc.Setjoint();
        // cout << "time:   "<< mc.fly_time <<endl;
        if (mc.fly_time >mc.T - 0.0005 && mc.state_val == 1)
          StateVal = mc.stateval[3];
        else if (mc.fly_time > mc.T - 0.0005 && mc.state_val == 2)
          StateVal = mc.stateval[2];
        else if (mc.fly_time > mc.T - 0.0005)
          StateVal = mc.stateval[3];
        cout << "b"<< endl;
        break;
      case 'c'://3 leg stance
        mc.stance_VMC_3leg();
        mc.swing_VMC_3leg();
        mc.Setjoint();
        if (mc.fly_time > mc.T - 0.0005 || mc.state_val == 1)
          StateVal = mc.stateval[3];
        cout << "c"<< endl;
        break;
      case 'd'://4 leg stance
        mc.stance_VMC_4leg();
        mc.Setjoint();
        static int temp_num = 0;
        if (temp_num == 3 && mc.translation <= 0.5)
        {
            StateVal = mc.stateval[1];
            if (mc.swingFlag == 0)
            {
              mc.swingFlag = 1;
              mc.fly_time = 0.0;
            }
            else
            {
              mc.swingFlag = 0;
              mc.fly_time = 0.0;
            }
            mc.setCoMVel(mc.tCV);
            temp_num = 0;
        }
        else if (temp_num == 50 && mc.translation == 1)
        {
          StateVal = mc.stateval[4];
          mc.fly_time = 0.0;
          temp_num = 0;
        }
        temp_num += 1;
        // oFile << mc.leg_force[0]<<","<< mc.leg_force[1]<<","<< mc.leg_force[2]<<","<< mc.leg_force[3]<<","<< mc.leg_force[4]<<","<< mc.leg_force[5]<<","<< mc.leg_force[6]<<","<< mc.leg_force[7]<<","<< mc.leg_force[8]<<","<< mc.leg_force[9]<<","<< mc.leg_force[10]<<","<< mc.leg_force[11]<< endl;
        cout << "d" <<endl;
        break;
      case 'e'://translation
        mc.changePosition_swing();
        mc.changePosition_stance();
        mc.Setjoint();
        if (mc.temp_judge == 0)
        {
          if (mc.surmountFlag == 3 && mc.fly_time > mc.T + 0.005)
          {
            StateVal = mc.stateval[3];
            mc.translation = 0;
            mc.temp_judge = 1;
            // mc.swingFlag = 1;
          }
          if (mc.fly_time > mc.T + 0.005)
          {
            mc.surmountFlag = mc.surmountFlag + 1;
            mc.fly_time = 0.0;
            if (mc.surmountFlag > 3)
            mc.surmountFlag = 0;
          }
        }
        else
        {
          if (mc.surmountFlag == 3 && mc.fly_time > mc.T + 0.005)
          {
            StateVal = mc.stateval[3];
            mc.translation = 0;
            mc.temp_judge = 0;
            // mc.swingFlag = 1;
          }
          if (mc.fly_time > mc.T + 0.005)
          {
            mc.surmountFlag = mc.surmountFlag + 1;
            mc.fly_time = 0.0;
            if (mc.surmountFlag > 3)
            mc.surmountFlag = 0;
          }
        }
        break;
      default:
      break;
    }
    cout << "v:   "<<mc.tCV.transpose()<<endl;
    cout << "imu:  "<<mc.imu_num.transpose()<<endl;
    cout << "trans:  "<<mc.translation<< endl;
    // cout << "imu:   "<< mc.imu_num.transpose()<<" "<< mc.target_taoz<<endl;
    // cout << "dis: "<<mc.distance.transpose()<<endl;
    // cout << "global: "<< mc.global_xyz.transpose() <<endl;
    // oFile << mc.distance[0] << "," << mc.distance[1] << "," << mc.distance[2] << "," << mc.distance[3] << "," << mc.distance[4] << "," << mc.distance[5] << "," << mc.distance[6] << "," << mc.distance[7] << endl;
    // oFile << mc.presentCoMVelocity[0] << "," << mc.presentCoMVelocity[1] << "," << mc.presentCoMVelocity[2] << "," << mc.targetCoMVelocity[0] << "," <<  mc.targetCoMVelocity[1] <<"," << mc.targetCoMVelocity[2] << "," << mc.imu_num[0] << "," << mc.imu_num[1] << "," << mc.imuVel
    // [2] << endl;
  }
oFile.close();
delete robot;
return 0;
}
